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SENSOR FUSION SYSTEM AND METHOD FOR ESTIMATING THE POSITION. 
SPEED AND ORIENTATION OF A VEHICLE. PARTICULARLY AN AIRCRAFT 

Field of the Invention 

5 This invention relates to a method and system for integrating data from different 

sensors and estimating the position, speed and orientation of a vehicle. It is particularly 
suited to being applied to Unmanned Aerial Vehicles (UAV) incorporating low-cost 
sensors for: 

- determining the attitude and orientation of the vehicle; 
10 - determining its position and speed; 

- navigating during limited time periods without GPS. 

The invention is comprised within the fields of sensor fusion, attitude 
determination and inertial navigation. 
Background of the Invention 
15 There are several methods for estimating the position, speed and orientation of 

a vehicle: 

1.- Inertial Navigation System or INS: Integrates the angular accelerations and 
velocities provided by an Inertial Measurement Unit (IMU) to calculate the position, 
speed and orientation of the vehicle. Since this integration is neutrally stable, errors 

20 accumulate and the obtained solutions quickly diverge unless very high quality sensors 
are used, increasing system cost and weight. 

The solution is to stabilize the integration by means of closed loop feedback 
with measurements from other sensors that do not diverge over time. Airspeed 
measurements and the measurements provided by GPS and magnetometers are used 

25 for this purpose. Several methods are used to integrate all or part of these 
measurements: 

1 .1 Linear Kalman Filter: It is the simplest filter with the lowest computational cost and 
therefore very interesting for low-cost applications. The drawback is that it is applicable 
only to linear or linearized dynamical systems. Therefore it can only be used in certain 
30 cases. 

1.2.- Complementary Kalman Filter: INS algorithms are used to integrate the 
measurements of an IMU that may be a low-cost IMU. The INS inputs are corrected 
with the outputs of a linear Kalman filter consisting of an INS and measurement error 
model and fed by the error between the position and speed estimated by the INS and 
35 the measurements by the remaining sensors. The drawback is that linearization leading 
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to the error model means that the global convergence is not assured and spurious 
updates could lead to system divergence; furthermore, like all Kalman filters its design 
implies knowledge of statistics of both the measurement noise and process noise and 
that these noises are white, Gaussian and non-correlated noises, which in the case of 
5 low-cost sensors occurs rather infrequently. Its computational cost ranges between 
moderate to high, depending on the size of the state vectors and measurements. 
1 .3.- Extended Kalman Filter: This is probably the most widely used filter as it is more 
precise than the standard Kalman filter. It can estimate the vehicle dynamics which is 
generally not linear because it allows non-linear terms both in the model and in the 

10 measurements. It has a higher risk of divergence than the standard Kalman because 
the covariance equations are based on the linearized system and not on the real non- 
linear system. Its asymptotic local stability has been proven, but its global stability 
cannot be assured. In addition to sharing with the remaining Kalman filters the need to 
know the noise and measurement statistics, its computational cost is high. 

15 2.- Static or Single Frame Method: Unlike the previous filtering methods, a static 
estimation is carried out consisting of obtaining the orientation from a set of unit vector 
measurements in both body axes and reference axes. Almost all logarithms 
(Davenport's, QUEST, FOAM,...) are based on resolving the Wahba problem which 
consists of finding an orthogonal matrix with a +1 determinant minimizing a cost 

20 function made up of the weighted sum of error squares between the unit vectors in 
body axes and the result of transforming the vectors into reference axes by the matrix 
that is sought. It is usually applied in space systems in which the unit vectors are 
obtained by pointing at the sun and other stars. 

There are some examples of the application to a dynamic estimation but it is 

25 based on its integration in a linear or complementary Kalman filter with the previously 
discussed drawbacks. 
Description of the Invention 

The invention relates to a system according to claim 1 and to a method 
according to claim 7. Preferred embodiments of the system and method are defined in 

30 the dependent claims. 

Given the shortcomings of the prior art, which can be summarized as: 

• no assurance of global convergence; 

• need to know noise and process statistics; 

• need for the measurement and process noises to be white, Gaussian and 
35 non-correlated; 
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• moderate to high computational cost; 

the object of this invention consists of providing a sensor fusion system according to 
claim 1 and a method according to claim 7 which: 

• is globally convergent in the ISS (Input-to-State Stability) sense; 
5 • does not require complex statistical models; and 

• has a low computational cost. 

To attain the aforementioned objectives it is necessary to solve the problem of 
stabilizing the integration of kinematic equations in a global manner without being 
computationally costly. To do so, rather than using statistical estimation methods such 
10 as in the Kalman filter this invention approaches the problem as a non-linear control 
problem and solves it by making use of the Lyapunov stability theory, finding a control 
law that provides the corrections u m which have to be applied to the angular velocity to 

stabilize the integration of the kinematic equations in an asymptotic global manner. 

The approach of the non-linear control problem and its solution has been 
1 5 developed as follows: 

a.- Approach: g b ,e b are two constant unit vectors expressed in vehicle-fixed axes 
(body axes) and g n e t are the same vectors expressed in local axes (Earth's axes). 
If B is the direction cosine matrix, then: 

Sb = Bg, 
e b = Be t 

20 By integrating kinematic equations with the measured angular velocity, cb h , a 

direction cosine matrix B would be obtained, which would be known to be erroneous 
due to sensor imprecision. Therefore, a correction u m will be applied to the measured 

angular velocity. Referring to cb b = & b +u G> as the corrected angular velocity and by 

integrating it, the direction cosine matrix B will be obtained. Estimated vectors g b ,e b 
25 can be obtained from this direction cosine matrix: 

g b =Bg t 
e b = Be t 

Now it is necessary to determine the correction u w to cancel out the error 

Sb = Sb~ Sb n 11 

e b ~ e b ~~ e b 

This is a non-linear control problem that is going to be solved using an 
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extension of the Lyapunov function concept referred to as Lyapunov Control Function 
(Clf) [ref. 1] in the following steps: 
1 . Forming the system 



z = f(z,u Q) \z = 



,/(0,0) = 0 [1.2] 



2. Finding a Clf for the previous system, i.e. a continuous derivable, positive- 
definite and non-radially bounded function J: 6 -> + such that: 

inf{^(z)/(z, W j)<0, Vz*0 [1.3] 



10 3. Finding a control law u Q) = a(z) such that if W(z) is a positive-definite 

function, the following is satisfied: 

—(z)f( zM z))<-W(z) [1.4] 

4. The LaSalle-Yoshizawa Theorem [ref. 1] assures that system [1.2] with 
15 u m -a{z) is globally uniformly asymptotically stable (GUAS) for the 

equilibrium point z=0. 
b.- Solution: 

1 . The following system is obtained by deriving [1.1]: 



e b =-<t> b *e b + u<»* e b 



2. The following Clf will be tried: 



[1.5] 



Az) = 7;[g b 'g b + e b e b ] [1.6] 



the derivative of which according to the solution of [1 .5] is: 

dJ , x 

— (z)/(z,*J = 
az 

25 -g b <& b xg b ) + g b [17] 
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Taking into account the vector product properties: 

— (z)/(z, u b ) = -u m • (g b xg b + e b xe b ) [1 .8] 

3. The following control law is used: 

5 where a is a positive scalar. 

Calling f = g b xg b +e b xe b and substituting in [1.8], the following is 
obtained: ~ 

^( z )f(z 9 uJ = -(T(T .f) = -<7|r| 2 [1.10] 

4. Given that r 2 is a positive-definite function, [1.4] is true and system [1.5] 
10 with u a =a(z) is globally uniformly asymptotically stable (GUAS) for 

equilibrium point 2=0 according to the LaSalle-Yoshizawa theorem [ref. 1]. 
c- Robustness: The previous Control Law meets the proposed objectives but only if 
perfect measurements of the g b ,e b vectors are available, which in practice would 

make it useless. However, as will be shown below this is not the case and this control 
15 law is still valid with erroneous measurements provided that they are bounded, as 
occurs in common practice. 

Suppose that measurements g b ,e b contain additive errors. The real vector will 
be the one measured plus the measurement errors: 

g = g + g 
e = e +e 

20 By substituting in the definition of f , the following is considered: 

F = (ft+&)Xft+(^+e A )xe, 
= [(ftXft) + («*xe 4 )] 

+ [(&x&) + («*x«*)J 

= f + f 

As only the measured value of f will be known, only f can be used in the 
control laws and equation [1.10] is modified to be: 

|^(^)/(^«J = -tT((f-f) f) 

dz [1.13] 

l-|2 



= -<r|fj +<r(f f) 
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so it can no longer be assured that system [1.5] with = of is globally uniformly 

asymptotically stable (GUAS) for the equilibrium point z=0. However, it will be 
demonstrated that it is ISS (Input-to-State Stable) [ref. 2], [ref. 3] and [ref. 1], i.e. the 
following property is satisfied: 

|z(O|<A|z(0)| 9 O + r(sup|f(r)|) [1.14] 

where p is a class KL function and y is a class Kf unction [ref. 1]. 
By using Young's inequality, the following is considered: 



r f <f f +-(r r) [1.15] 

4 



1 0 and substituting in [1 . 1 3]: 



^(z)/(z, W J < -0.75(j|r| 2 + <r|ff [1 .16] 

proving that y"00 / 0>*O < 0, Vz * 0 , provided that |rf > ^|F|| 2 . 

As J(z) and r 2 (z) are positive-definite and are not radially bounded and J(z) 
is smooth, there are class K„ Y\*YnYz functions such as: 

15 . r x <tf)zJiz)zrM [117] 

r 3 (H)^ r2 ^) 

Therefore, 

NO|>rr , [j||r||l].^^<o [1.18] 



Which means that if: 



20 then: 



which in turn means that 



r(o| < ^ o r 2 o rr 1 (f 11*111) t 1 - 19 J 



If on the other hand 
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then 

\z(0\<y; l o r2 (\z(0)\) [1.20] 

Therefore, taking into account [1.19] and [1.20], it is shown that z(t) is globally 
5 bounded: 

\\z\l < maxjr- o y 2 . y* (±|f£). . r 2 (|z(0)|)} 

and converges to the residual set: 




[1.21] 



Furthermore, by Theorem C.2 [ref. 1] (formulated in [ref. 2]), system [1.5] with 
10 Control Law u w = of is ISS with respect to the errors of the sensors f . 

Therefore, this invention relates to a system for estimating the position, speed 
and orientation of a vehicle, such as an unmanned aircraft, including: 

means for determining the components of two noncollinear constant unit vectors 
g b ,e b according to vehicle body axes; 

15 - means for determining the components of said noncollinear constant unit 
vectors g ( ,e t according to the Earth's axes; and 

means for determining the three components of the angular velocity S) b of the 
vehicle in body axes. 

The sensor fusion system of the invention further includes: 
20 - means for correcting the angular velocity S> b with a correction u m and obtaining 

a corrected angular velocity cb b = S> b + ; 

a module for integrating the kinematic equations of the vehicle receiving the 
corrected angular velocity 8> h as input and providing the transformation matrix B for 
. transforming Earth's axes into vehicle body axes and the orientation of the vehicle in 
25 the form of Euler angles 4> ; 

a synthesis module of the components in body axes of the two noncollinear 
constant unit vectors to provide an estimation of said noncollinear vectors in body axes 
g b ,e b , where said estimation is calculated as follows: 
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g b =Bg t 
e b =Be t 

a control module implementing a control law to calculate said correction w^,, 
where said control law is: 

W a>=^(£6 X £6+^ X ^) [1] 

5 where a is a positive scalar, 

such that by applying this correction u m to the measured angular velocity 5> b and using 
the resulting angular velocity cb b = d) b + u m as input to the module for integrating the 
kinematic equations, the latter are stable in the ISS sense and the error in the 
estimation of the direction cosine matrix B and of the Euler angles O is bounded. 
10 Said noncollinear unit vectors g,e are preferably local gravity g and projection 

of the magnetic field on the plane perpendicular to gravity e . 

The means for determining the components of the two noncollinear constant 
unit vectors g b ,e b according to vehicle body axes preferably include: 

an inertial measurement unit (IMU) including a group of at least three 
15 gyroscopes and at least three accelerometers located along the vehicle body axes; 

a magnetometer with the ability to measure the Earth's magnetic field according 
to the vehicle body axes; 

static and differential pressure sensors; 
two vanes to measure the angles of attack and sideslip; 
20 - an angular velocity acquisition and processing module configured to obtain the 
angular velocity a> b (t) measured by the gyroscopes and delaying it to obtain S) b {t-r)\ 
a data acquisition and processing module configured to acquire the specific 
force a b (t) measured by the accelerometers, the static pressure p s (t), the differential 

pressure p d (t), the angle of attack a(f), the angle of sideslip /?(0.and the value of 
25 the Earth's magnetic field fh b (t) measured in the magnetometer, delay them and 
process them to calculate the true airspeed v(r-r), the airspeed in body axes 
V*(f — t) as follows: 

C cos a cos P 
v h ~ vsinfl 
C sin 5 cos ft 
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to calculate the numerical derivative of the airspeed in body axes v b (t-r) , 
to calculate the local gravity in body axes g b as follows: 

&(' - r) = v b (t - r) + & b (t - r)xv b (t - t) - - r) 

and to calculate the projection of the Earth's magnetic field on the horizontal plane 
5 perpendicular to local gravity e(j-r) as follows: 

e b (t- r) = fh b {t - r) - m b (t - r) . 

The means for knowing the components of the two noncollinear constant unit 
vectors g n e t according to the Earth's axes can include: 

a GPS (Global Positioning System) receiver, 
10 and the data provided by the GPS are acquired, processed and used in said data 
acquisition and processing module (17) to calculate the components of the two 
noncollinear constant unit vectors g n e t according to the Earth's axes. 

The system can preferably include a Savitzky-Golay filter in which v b , numerical 
derivative of v b , is calculated. 

15 According to an additional aspect of this invention, a sensor fusion system is 

provided for estimating position and speed of a vehicle, particularly an unmanned 
aircraft, preferably further including: 

means of acquiring data from a group of sensors located in the vehicle, such as 
a GPS (Global Positioning System) receiver and a static pressure sensor which provide 

20 position and speed in Earth-fixed axes P n V t ; 

in the event that the means for knowing the components of the previously 
mentioned constant unit vectors according to the vehicle body axes did not include 
data acquisition means of a group of three accelerometers located along the vehicle 
body axes, the latter, will be included to provide the specific force a b in body axes; 
25 - a navigation module where the vehicle navigation equations are integrated from 
the specific force a b and the direction cosine matrix B to obtain calculated position 

and speed in local axes and are corrected in a Kalman filter, which can be static, to 
obtain estimated position and speed in local axes. 

In the case of an unmanned aircraft all these requirements of means for 
30 knowing different parameters or for sensor data acquisition can be met with the 
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sensors usually provided in this type of aircrafts, as will be seen in the coming sections, 
and therefore the amount of sensors needed is not a drawback of this invention, but 
quite the opposite, it is an advantage insofar as it extracts maximum information from 
the sensors that it has. 

5 Another object of this invention is to provide a sensor fusion method for 

estimating the position, speed and orientation of a vehicle comprising: 

calculating the components of two noncollinear constant unit vectors g b ,e b 

according to vehicle body axes from measurements of sensors located in the vehicle 
according to the body axes of the latter; 
10 - calculating the components of said noncollinear constant unit vectors g t9 e t , 

according to the Earth's axes from measurements of sensors located in the vehicle 
which provide position in local axes; 

measuring the three components of angular velocity 8) b of the vehicle in body 

axes; 

15 - correcting the angular velocity S) b with a correction u m and obtaining a 
corrected angular velocity 6) b - S> b + ; 

integrating the kinematic equations of the vehicle, according to the corrected 
angular velocity &> b , and providing the transformation matrix B for transforming the 
Earth's axes into vehicle body axes and the orientation of the vehicle in the form of 
20 Euler angles <f> ; 

calculating an estimation of the components in body axes of the two 
noncollinear constant unit vectors g b9 e b , where said estimation is calculated as follows: 

it = 
e b = Be t 

obtaining the correction by means of the control law: 

25 u„=<r(g b xg b + e b xe b ) [1] 

where a a positive scalar, 

such that upon applying this correction u a to the measured angular velocity 5> b and 
using the resulting angular velocity cb b = d> b + u m as input to the module for integrating 
the kinematic equations, the latter are stable in the ISS sense and the error in the 
30 estimation of the direction cosine matrix B and of the Euler angles <f> is bounded. 
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15 Brief Description of the Drawings 

A series of drawings which aid in better understanding the invention and which 
are expressly related to an embodiment of said invention, presented as a non-limiting 
example thereof, are very briefly described below. 

Figure 1 shows in a block diagram the system of the invention in a navigation, 
20 guidance and control system in an unmanned aerial vehicle (UAV). 

Figure 2 shows the implementation of the system of the invention as an 
integrated element of a flight control system. 

Figure 3 shows a general block diagram of the elements forming the system of 
the invention. 

25 Figure 4 shows a block diagram of a detailed view of the kinematic equation 

solution module. 

Figure 5 shows a detailed block diagram of the angular velocity acquisition and 
processing and the sensor data acquisition and processing modules. 

Figure 6 shows a detailed block diagram of the navigation module. 
30 Description of a Preferred Embodiment of the Invention 

As an example of the best way to carry out the invention to practice, the 
embodiment of a sensor fusion unit is shown for a flight control system of a short-mid 
range, low speed unmanned aerial vehicle (UAV) without this ruling out the possibility 
of there being other implementations. 
35 Figure 1 shows the flight control system in an unmanned aerial vehicle 10 
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consisting of sensors 2, the system 1 of the invention, a control and guidance system 3 
and actuators 4. The system 1 of the invention supplies information on the attitude, 
speed and position to the control and guidance system 3, which in turn provides the 
corresponding information to the actuators 4 of the aircraft. The information provided by 
5 a group of sensors, which in Figure 1 are depicted together in a single box as sensors 
2, are inputs of the system of the invention. 

Figure 2 details said sensors which are: 

an Inertial Measurement Unit (IMU) 21, including a group of three gyroscopes 
21 1 and three accelerometers 212 located along the vehicle body axes (in this case it 
10 is a low-cost solid state IMU, for example Crossbow IMU400); 

a magnetometer 22 capable of measuring the Earth's magnetic field according 
to the three vehicle body axes; 

a Pitot tube 23 including static pressure 231 and differential pressure 232 
sensors; 

15 - two vanes 24 provided with potentiometers to measure the angles of attack 241 
and sideslip 242; 

a GPS (Global Positioning System) receiver 25 with a single antenna. 
The best contemplated embodiment of the system of the invention is a 
microprocessor-based system made up of PC-104 cards. As is shown in Figure 2, the 

20 system consists of an analog-digital acquisition card 26 (for example, DM6420 by RTD 
Embedded Technologies) for acquiring data from the magnetometer 22, the vanes 24 
and the Pitot 23, a RS-232 serial port card 29 (for example, CM310 by RTD Embedded 
Technologies) for acquiring data from the IMU 21 and from the GPS 25, a CPU card 28 
(for example, CMV6486 by RTD Embedded Technologies) containing the 

25 microprocessor and the memory where the algorithms of modules 11, 12, 13, 14, 15, 
16 and 17 (Figure 3) are stored in the form of an executable program. The system has 
a PC-104 bus 20 communicating with the different cards and an electric power supply 
card 27 having as input a standard 28-volt airplane power supply and generating and 
distributing the +5V,±\2V needed by the cards through buses. 

30 In this embodiment the directions of local gravity g and of the projection of the 

Earth's .magnetic field on the horizontal plane perpendicular to local gravity e , 
considered constants, are used as noncollinear constant unit vectors. Both in body 
axes and in the Earth's axes, the components of these vectors are calculated from 
previously mentioned sensor measurements in the sensor data acquisition and 

35 processing module 17. 
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So to calculate g b , the specific force a b {t) measured by the accelerometers 
212 is acquired in module 17 and delayed a time r, corresponding to five sampling 
periods in the FIFO 170 to obtain a b (t-z) and supply it to module 178. Similarly, the 
angular velocity & b (t) measured by the gyroscopes 211 is acquired in the angular 
velocity acquisition and processing module 1 1 and is delayed in the FIFO 111 to obtain 
&> b (t-T) which is supplied to module 178. The static pressure p s measured in sensor 

231 and the differential pressure p d measured in sensor 232 are acquired in module 17 
and processed in the altitude and speed calculation module 171 to obtain v the true 
airspeed (TAS) and the altitude h by a process that is known for a person skilled in the 
art. As this process requires knowing the outside ambient temperature T 0 , such 
temperature is initially provided before the flight, for example in an initialization file. The 
true airspeed v is supplied to module 176 together with the angle of attack a 

acquired from sensor 241 , and the angle of sideslip p acquired from sensor 242, and 
the airspeed vector in body axes v b (j) is calculated in said module 176 as follows: 

v cos a cos ft 
v h = vsinfi 
C sin a cos /? 

This speed is delayed a time r to obtain v b (t~-r) which is supplied to module 
178. The speed v b (t) is numerically derived by using a Savitzky-Golay filter 179 [ref. 6] 

of degree four with a window of 11 samples, the derivative for the sample being 
calculated with index 5, i.e. the derivative is delayed t, five sampling periods. This 

derivative v b {t-r) is supplied to module 178 where the components of the local 

gravity vector g b (t—r) in body axes are finally calculated as follows: 

g b {t - t) = v b (t - r) + & h (t - r) x v b (t - r) - a b (t - r) 
which correspond to the translational equations of the movement of the vehicle with the 
hypothesis of "flat Earth", constant gravity, accelerometer located in the vehicle's 
center of gravity and constant wind speed. In view of these equations, the reason for 
delaying the different variables is justified. Obtaining the numerical derivative of speed, 
which is a variable that contains noise, is not an easy task; one of the best solutions is 
the Savitzky-Golay filter, but it is a non-causal filter so the moment in which the 
derivative is calculated must be delayed. 
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To calculate e b , the value of the Earth's magnetic field m b (t) measured in the 
magnetometer 22 is first acquired in module 17 and is delayed a time r in the FIFO 
174 to obtain m b (t — T) and supply it to module 175. The following calculation is 

performed in this module 175, which has also been provided with g b (t— r) : 

5 - e b (t - T ) = m b (t-T)- m b (t - r) ■ ^ - T) 

The calculation of the components of the unit vectors in Earth's axes in the 
direction of local gravity and the projection of the Earth's magnetic field on the 
horizontal plane perpendicular to local gravity g n e t is done in module 173. An 

International Geomagnetic Reference Field (IGRF) [ref. 4] model is implemented in this 
10 module 173 which provides the components of the Earth's magnetic field in Earth's 
axes m t given the position of the vehicle P t . A local gravity model provides g t in the 

same manner from the position of the vehicle P t . To obtain this position, the 
measurements of the GPS 25 are obtained in module 17, processed in module 172 to 
convert them from the GPS reference system to Earth's axes and the position P t thus 

1 5 obtained is supplied to module 1 73. 

The three gyroscopes 211 located along the aircraft body axes measure the 
three components of the angular velocity a> b in these axes. It is acquired in an angular 

velocity acquisition and processing module 11, where a FIFO 111 is delayed a time 
r corresponding to five sampling periods. Module 1 1 supplies the current angular 

20 velocity & b (t) to the kinematic equation solution module 13 and the delayed angular 
velocity & b (t-r) to module 12, where the correction ujj-r) calculated in control 
module 14 is added to it to obtain a corrected angular velocity 
^ ) b( t ~ t ) = &b(t~- T ) + u a>( t - T ) » which is also supplied to module 13. 

The quaternion q T = [q Q q x q 2 q 3 ] is used in the kinematic equation 

25 solution module 13 as an attitude representation means. The quaternion is initially 
calculated in module 134 from the initial Euler angles = 0 yr] supplied 
externally as shown below: 
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*<«-KtHfM*M*MfM?) 

9 A, 0 ) = cos(i)sin(|)cos^] + S in(A]cos(|)sin(^ ^ 

The kinematic equations of the movement of the vehicle expressed according to 
the quaternion are: 
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which shall be represented in abbreviated form as: q{t)-^{a) b {t))q{t) • These 
equations will be integrated in two steps: 

° t-r 

in a first step, the corrected angular velocity d) will be used in the differential equations 
[2.2] and will be integrated in module 137 using, for example, the McKern method [ref. 
5], from the initial time until t-T, because only cb b (t-T), not d) b (t) 9 is available in 
each instant t. q(t-r) is thus obtained and as was demonstrated when developing 
the control law, it is known that the error q(t-r) = q(J-T) — q(t-r) is bounded. The 
second step of the integration is done in module 132 where the derivative of the 
quaternion, calculated in module 131 using the angular velocity & b (t) , is integrated 
from instant t—T until t, and using as an initial value in r-rthe quaternion estimated 
in module 137: q(t-r) . Given that this second step of the integration is carried out 
during a very short time r , corresponding to five samples, the error committed by 
using 3> b (j) rather than d) b (t) is negligible and the obtained result is q(t) 9 which is 

distributed to module 138 where the transformation matrix B(t) for transforming the 
Earth's axes into body axes is calculated as shown below: 
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% +% -$ -% 2 {m -44) 2 im +44) 
2(44+44) $-#+$-$ 2(44+44) 
2(44 -44) 2(44-4^) $-#-$+$ 



using the quaternion g(0- B(t-t) is calculated in the same way in module 138 using 
q(t-f) provided by module 137. 

Once B{t-r) provided by module 137 and e,(f-r),£,(r-T) provided by 17 
5 are known, e b (t — f) 9 g b (t — r) is synthesized in module 15 as shown below: 

e b (t-T) = B(t-T)e t 

g b (t-T) = B(t-T)g t - 

and the previous correction u a (t-r) is calculated in module 14 by means of the 
control law: 

1 0 where a is a positive constant scalar gain which has been selected in this case at a 
value of 0.1. 

As is shown in Figure 6, the navigation module 16 consists of three stationary 
Kalman filters, one for each channel x, y, z, 161, 162, 163 respectively, and the 
acceleration in the Earth's axes is calculated in module 164 from the specific force 

15 a b (t) and gravity g t provided by module 17 and B(t) provided by module 13: 

a t = B T a b +g t 

and each component of a t is supplied as a control signal to each of the filters. Assume 

that 5, contains exponentially correlated noise that can be described as: 

a t =a t + r 
r =-yr + £ r 

20 where e r is white noise with zero mean and y is diagonal. Therefore, the basic state 

vector consisting of p t V t position and speed increases with r , corresponding to the 

previous shaping filter to take into account the correlation in acceleration. 

It is considered that the speed of the mass of air with respect to the Earth 

consists of one constant V w , the wind speed, and a correlated noise w g representing 

25 atmospheric turbulence and which can be represented by: 
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where £ s is white noise with zero mean and the constants Z,ju are determined from 
the spectral density specified in [Mil. Spec. 1797]. 

To identify the wind speed in the Earth's axes, the previously mentioned 
shaping filter will be incorporated adding three more components to the state vector of 
each of the filters: V w9 s 0 ,s x . Finally the state vector is: 



x i=[Pti K r t *oi * u V wi \ 



The state equations are: 

P, = v, 

V,=a,+r 
r = -yr + e r 
s 0 = s t 

K = o 

10 In addition to the GPS measurements p t ,V t , the airspeed in the Earth's axes 

will be incorporated: 

which is calculated in module 1 65. 

The measurement of the barometric height will also be incorporated but only in 
1 5 the filter corresponding to channel z: 

h-K=-Pa +J lH 
The measurement equations are: 

v, =V,-V w -^s 0 -^s x +T] v 

where 7j is white noise with zero mean. 
20 The Kalman filters are: 
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where: 



i = 0,1,2 
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and the gains AT, are obtained by solving the corresponding algebraic Riccati equation 
given the covariance matrixes of the process Q t and measurement R, noise: 

a,p, + pa t + a - ptfK'cf, = o 

i = 0,1,2 



